/**
 * 拍照程序
 */

#ifndef COMPOSITION__LISTENER_COMPONENT_HPP_
#define COMPOSITION__LISTENER_COMPONENT_HPP_

// #include "composition/visibility_control.h"
#include "libbase/common.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <cv_bridge/cv_bridge.hpp>
#include "ros_usbcapture/UsbCaptureSystem.h"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/camera_info.hpp"


namespace wmj
{

    class TakePhoto : public rclcpp::Node
    {
    public:
        // COMPOSITION_PUBLIC
        explicit TakePhoto(const rclcpp::NodeOptions &options) : Node("TakePhoto", options)
        {
            rmw_qos_profile_t qos_t;
            qos_t.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
            qos_t.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
            qos_t.depth = 5;
            qos_t.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
            qos_t.deadline = RMW_QOS_DEADLINE_DEFAULT;
            qos_t.lifespan = RMW_QOS_LIFESPAN_DEFAULT;
            qos_t.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
            qos_t.liveliness_lease_duration = RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT;
            qos_t.avoid_ros_namespace_conventions = false;
            rclcpp::QoSInitialization qosInitialization = rclcpp::QoSInitialization::from_rmw(qos_t);
            rclcpp::QoS qos = rclcpp::QoS(qosInitialization, qos_t);
            
            RCLCPP_INFO(rclcpp::get_logger("INFO"), "image_component get is working ");
            //创建订阅者
            sub_image = this->create_subscription<sensor_msgs::msg::Image>(
                                "image_raw", qos, std::bind(&TakePhoto::call_back, this, std::placeholders::_1));
            std::thread(&TakePhoto::takePhotoThread, this).detach();
        }

    private:

        void takePhotoThread()
        {
            RCLCPP_INFO(this->get_logger(), "输入要拍的照片张数: ");
            // std::cin >> limit;
            usleep(3e6);
            limit = 25;
            RCLCPP_INFO(this->get_logger(), "请按s键拍照 %d 次", limit);

            while(rclcpp::ok())
            {
                image_mutex.lock();
                cv::Mat image = GetImg(*frame);
                std::string orientation = frame->header.frame_id;
                image_mutex.unlock();

                if(image.empty())
                {
                    RCLCPP_INFO(this->get_logger(), "空图像");
                    continue;
                }

                cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
                cv::namedWindow(orientation, cv::WINDOW_NORMAL);
                cv::imshow(orientation, image);

                char c = cv::waitKey(1);
                if (c == 'q' || c == 27)
                    break;
                else if(c == 's')
                {
                    cv::imwrite(orientation + "_" + std::to_string(count) + ".jpg", image);
                    count++;
                    RCLCPP_INFO(this->get_logger(), "拍了 %d 次", count);
                }
                if(count == limit)
                    break;
            }
        }

        //声明回调函数
        void call_back(const sensor_msgs::msg::Image::SharedPtr msg)
        {
            // RCLCPP_INFO(rclcpp::get_logger("INFO"), "get image msg");
            //利用 GetImg 获取图像，传入类型为 sensor_msgs::msg::Image
            image_mutex.lock();
            frame = msg;
            image_mutex.unlock();

        }

        cv::Mat GetImg(sensor_msgs::msg::Image img)
        {
            // 声明 cv::image 指针，内含cv::Mat
            cv_bridge::CvImageConstPtr cv_ptr;
            // toCvShare() 使用
            // sensor_msgs::msg::Image::ConstPtr img_ptr;

            // 获取图像，最终图像通过 cv_ptr->image 获取，为cv::Mat型
            try
            {
                // 把msg里的 Image 转换为 cv::mat类型 , 有两种方式 toCvCopy() 和 toCvShare() ,toCvShare() 虽然可以实现直接引用不用拷贝，
                // 但需要创建 sensors_msgs::ImageConst 指针，经测试二者差别不大，故用toCvCopy().后续还可改进.

                cv_ptr = cv_bridge::toCvCopy(img);

                // toCvShare() 方法
                // img_ptr = std::make_shared<const sensor_msgs::msg::Image>(msg->image);
                // cv_ptr = cv_bridge::toCvCopy(img_ptr);
            }
            catch (cv_bridge::Exception &e)
            {
                printf("cv_bridge exception:%s", e.what());
            }
            return (cv_ptr->image);
        }

        int count = 0;
        int limit = 20;
        int conti_flag = 0;

        sensor_msgs::msg::Image::SharedPtr frame;
        std::mutex image_mutex;

        //声明发布者
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image;
        
    };

} // namespace composition


#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(wmj::TakePhoto)

#endif // COMPOSITION__LISTENER_COMPONENT_HPP_
